/***************************************************************************
 创建者: 林逢达
 开始时间: 2016.11.28
 copyright: (C) 同川科技
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 (1) 华磊。2017.6.13,　添加关节增量检查功能。
 ２）华磊。２017.6.21,安全检查不用脉冲单位，直接用rad ,m.

 ***************************************************************************
 *  @file InternalSafe.h
 *  功能: 算法安全检查
 *
 *                                                                         *
 ***************************************************************************/
#ifndef INTERNALSAFE_H
#define INTERNALSAFE_H

#include <qvector.h>
#include "domparser.h"
#include "GeneralDefine.h"
#include <kdl/jntarray.hpp>

using namespace KDL;

class  InternalSafe:  public DomParser
{
public:
    InternalSafe(QVector<JointAttribute> *_robotAttribute, QVector<MotorData> *_motorDataVector, double ethercatDeltaTimeIn);
    int jointLimitCheck(const JntArray& jointsPosition,  const JntArray &jointsPositionFeedback);//关节限位检查
    int jointVelCheck(const JntArray& jointsVel);//关节速度检查
    int jointIncrementCheck(std::vector<double> jointIncrement);//关节增量检查
    int jointIncrementIncrementCheck(std::vector<double> jointIncrementIncrement);//关节速度增量检查
    int setControlType(int controlTypeIn); //设置　Ｔ１　Ｔ２　ＡＵＴＯ　ＥＸＴＥＲＮ
    int maxMotorPowerCheck(std::vector<double> motorPower);//电机功率检查

private:
    friend class McControllerInterpreter;
//    QVector<JointAttribute> *robotAttribute;//机器人关节属性指针
    QVector<MotorData> *motorDataVector;
    int jointsNum;
    std::vector<double> positiveLimit;
    std::vector<double> negativeLimit;
    std::vector<double> speedLimit;
    std::vector<double> jogSpeedLimit;
    std::vector<double> accelerationLimit;
    double ethercatDeltaTime;// units is S
    std::vector<double> jointIncrementLimit;
    std::vector<double> jointIncrementIncrementLimit;
    std::vector<double> jogJointIncrementIncrementLimit;
    std::vector<double> jogJointIncrementLimit;


    /**
     * @brief controlType
     */
    volatile int controlType;//20170111改为控制模式(速度限制)0 T1 　 1 T2 　 2 自动 　 3 外部
protected:
    std::vector<int> stopJointList; //超限位标识
    std::vector<int> overspeedList;//超速度标识
    std::vector<int> jointIncrementAlarmList; //超关节增量标识
    std::vector<int> jointVelIncrementAlarmList; //超关节速度增量标识
};

#endif // INTERNALSAFE_H
